﻿#pragma once
#include "AsMathBasic.h"
#include "AsMathPlane.h"
#include "AsMathInterval.h"

namespace AsMath {

	template <typename _Type>
	struct MatrixParams {
		_Type		filterZLow;
		_Type		filterZUp;
		cv::Rect	roi = cv::Rect(-1, -1, -1, -1);
		bool		enableZFilter;
		static MatrixParams ZRange(_Type filterZLow, _Type filterZUp) {
			MatrixParams matrixParams;
			matrixParams.filterZLow = filterZLow;
			matrixParams.filterZUp = filterZUp;
			matrixParams.enableZFilter = true;
			return matrixParams;
		}
		static MatrixParams Range(cv::Rect roi) {
			MatrixParams matrixParams;
			matrixParams.roi = roi;
			matrixParams.enableZFilter = false;
			return matrixParams;
		}
		static MatrixParams Range(cv::Rect roi, _Type filterZLow, _Type filterZUp) {
			MatrixParams matrixParams;
			matrixParams.filterZLow = filterZLow;
			matrixParams.filterZUp = filterZUp;
			matrixParams.roi = roi;
			matrixParams.enableZFilter = true;
			return matrixParams;
		}
		static MatrixParams Row(int row) {
			MatrixParams matrixParams;
			matrixParams.roi.x = -1;
			matrixParams.roi.width = -1;
			matrixParams.roi.y = row;
			matrixParams.roi.height = 1;
			matrixParams.enableZFilter = false;
			return matrixParams;
		}
		static MatrixParams Row(int row, _Type filterZLow, _Type filterZUp) {
			MatrixParams matrixParams;
			matrixParams.filterZLow = filterZLow;
			matrixParams.filterZUp = filterZUp;
			matrixParams.roi.x = -1;
			matrixParams.roi.width = -1;
			matrixParams.roi.y = row;
			matrixParams.roi.height = 1;
			matrixParams.enableZFilter = true;
			return matrixParams;
		}
		static MatrixParams Column(int col) {
			MatrixParams matrixParams;
			matrixParams.roi.x = col;
			matrixParams.roi.width = 1;
			matrixParams.roi.y = -1;
			matrixParams.roi.height = -1;
			matrixParams.enableZFilter = true;
			return matrixParams;
		}
		static MatrixParams Column(int col, _Type filterZLow, _Type filterZUp) {
			MatrixParams matrixParams;
			matrixParams.filterZLow = filterZLow;
			matrixParams.filterZUp = filterZUp;
			matrixParams.roi.x = col;
			matrixParams.roi.width = 1;
			matrixParams.roi.y = -1;
			matrixParams.roi.height = -1;
			matrixParams.enableZFilter = true;
			return matrixParams;
		}
		static MatrixParams RowInterval(int row, const AsMathInterval& itv) {
			MatrixParams matrixParams;
			matrixParams.roi.x = itv.getMin();
			matrixParams.roi.width = itv.getLength();
			matrixParams.roi.y = row;
			matrixParams.roi.height = 1;
			matrixParams.enableZFilter = false;
			return matrixParams;
		}
		static MatrixParams ColumnInterval(int column, const AsMathInterval& itv) {
			MatrixParams matrixParams;
			matrixParams.roi.x = column;
			matrixParams.roi.width = 1;
			matrixParams.roi.y = itv.getMin();
			matrixParams.roi.height = itv.getLength();
			matrixParams.enableZFilter = false;
			return matrixParams;
		}
		static MatrixParams All() {
			MatrixParams matrixParams;
			matrixParams.enableZFilter = false;
			return matrixParams;
		}
	};

};

namespace AsMath {

	void AS_MATH_API getRange(cv::Mat inputMat, const cv::Rect& roi, int& xBegin, int& yBegin, int& xEnd, int& yEnd);

	cv::Mat AS_MATH_API shortenToLeft(cv::Mat inputMat, int shortenLength);
	cv::Mat AS_MATH_API shortenToRight(cv::Mat inputMat, int shortenLength);
	cv::Mat AS_MATH_API shortenToUp(cv::Mat inputMat, int shortenLength);
	cv::Mat AS_MATH_API shortenToDown(cv::Mat inputMat, int shortenLength);

	void AS_MATH_API threshold(const cv::Mat& inputMat, cv::Mat& outputMat, int thresh, bool inversion = false);

};

namespace AsMath {

	template <typename _Type, typename _Functor>
	void traverse(cv::Mat& inputMat, const MatrixParams<_Type>& matrixParams, _Functor functor) {
		int xBegin, yBegin, xEnd, yEnd;
		getRange(inputMat, matrixParams.roi, xBegin, yBegin, xEnd, yEnd);
		const _Type filterZLow = matrixParams.filterZLow;
		const _Type filterZUp = matrixParams.filterZUp;
		_Type* ptr;
		if (matrixParams.enableZFilter) {
			for (int y = yBegin; y < yEnd; ++y) {
				ptr = inputMat.ptr<_Type>(y, xBegin);
				for (int x = xBegin; x < xEnd; ++x, ++ptr) {
					if (*ptr >= filterZLow && *ptr <= filterZUp) {
						functor(*ptr);
					}
				}
			}
		}
		else {
			for (int y = yBegin; y < yEnd; ++y) {
				ptr = inputMat.ptr<_Type>(y, xBegin);
				for (int x = xBegin; x < xEnd; ++x, ++ptr) {
					functor(*ptr);
				}
			}
		}
	}

	template <typename _Type, typename _Functor>
	void traverse_nofilter(cv::Mat& inputMat, const MatrixParams<_Type>& matrixParams, _Functor functor) {
		int xBegin, yBegin, xEnd, yEnd;
		getRange(inputMat, matrixParams.roi, xBegin, yBegin, xEnd, yEnd);
		_Type* ptr;
		for (int y = yBegin; y < yEnd; ++y) {
			ptr = inputMat.ptr<_Type>(y, xBegin);
			for (int x = xBegin; x < xEnd; ++x, ++ptr) {
				functor(*ptr);
			}
		}
	}

	template <typename _Type, typename _Functor>
	void traverse_const(const cv::Mat& inputMat, const MatrixParams<_Type>& matrixParams, _Functor functor) {
		int xBegin, yBegin, xEnd, yEnd;
		getRange(inputMat, matrixParams.roi, xBegin, yBegin, xEnd, yEnd);
		const _Type filterZLow = matrixParams.filterZLow;
		const _Type filterZUp = matrixParams.filterZUp;
		const _Type* ptr;
		if (matrixParams.enableZFilter) {
			for (int y = yBegin; y < yEnd; ++y) {
				ptr = inputMat.ptr<_Type>(y, xBegin);
				for (int x = xBegin; x < xEnd; ++x, ++ptr) {
					if (*ptr >= filterZLow && *ptr <= filterZUp) {
						functor(*ptr);
					}
				}
			}
		}
		else {
			for (int y = yBegin; y < yEnd; ++y) {
				ptr = inputMat.ptr<_Type>(y, xBegin);
				for (int x = xBegin; x < xEnd; ++x, ++ptr) {
					functor(*ptr);
				}
			}
		}
	}
	
	template <typename _Type, typename _Functor>
	void traverse_xyz(const cv::Mat& inputMat, const MatrixParams<_Type>& matrixParams, _Functor functor) {
		int xBegin, yBegin, xEnd, yEnd;
		getRange(inputMat, matrixParams.roi, xBegin, yBegin, xEnd, yEnd);
		const _Type filterZLow = matrixParams.filterZLow;
		const _Type filterZUp = matrixParams.filterZUp;
		const _Type* ptr;
		if (matrixParams.enableZFilter) {
			for (int y = yBegin; y < yEnd; ++y) {
				ptr = inputMat.ptr<_Type>(y, xBegin);
				for (int x = xBegin; x < xEnd; ++x, ++ptr) {
					if (*ptr >= filterZLow && *ptr <= filterZUp) {
						functor(x, y, *ptr);
					}
				}
			}
		}
		else {
			for (int y = yBegin; y < yEnd; ++y) {
				ptr = inputMat.ptr<_Type>(y, xBegin);
				for (int x = xBegin; x < xEnd; ++x, ++ptr) {
					functor(x, y, *ptr);
				}
			}
		}
	}

	template <typename _Type>
	double calcSum(const cv::Mat& inputMat, const std::vector<cv::Point>& positions) {
		double sum = 0;
		for (const auto& position : positions) {
			sum += *inputMat.ptr<_Type>(position.y, position.x);
		}
		return sum;
	}

	template <typename _Type>
	double calcSum(const cv::Mat& inputMat, const MatrixParams<_Type>& matrixParams) {
		int xBegin, yBegin, xEnd, yEnd;
		getRange(inputMat, matrixParams.roi, xBegin, yBegin, xEnd, yEnd);
		const _Type filterZLow = matrixParams.filterZLow;
		const _Type filterZUp = matrixParams.filterZUp;
		double accumulation = 0;
		const _Type* ptr;
		if (matrixParams.enableZFilter) {
			for (int y = yBegin; y < yEnd; ++y) {
				ptr = inputMat.ptr<_Type>(y, xBegin);
				for (int x = xBegin; x < xEnd; ++x, ++ptr) {
					if (*ptr >= filterZLow && *ptr <= filterZUp) {
						accumulation += *ptr;
					}
				}
			}
		}
		else {
			for (int y = yBegin; y < yEnd; ++y) {
				ptr = inputMat.ptr<_Type>(y, xBegin);
				for (int x = xBegin; x < xEnd; ++x, ++ptr) {
					accumulation += *ptr;
				}
			}
		}
		return accumulation;
	}

	template <typename _Type>
	double calcMean(const cv::Mat& inputMat, const MatrixParams<_Type>& matrixParams) {
		int xBegin, yBegin, xEnd, yEnd;
		getRange(inputMat, matrixParams.roi, xBegin, yBegin, xEnd, yEnd);
		const _Type filterZLow = matrixParams.filterZLow;
		const _Type filterZUp = matrixParams.filterZUp;
		double accumulation = 0;
		int count = 0;
		const _Type* ptr;
		if (matrixParams.enableZFilter) {
			for (int y = yBegin; y < yEnd; ++y) {
				ptr = inputMat.ptr<_Type>(y, xBegin);
				for (int x = xBegin; x < xEnd; ++x, ++ptr) {
					if (*ptr >= filterZLow && *ptr <= filterZUp) {
						accumulation += *ptr;
						count++;
					}
				}
			}
		}
		else {
			for (int y = yBegin; y < yEnd; ++y) {
				ptr = inputMat.ptr<_Type>(y, xBegin);
				for (int x = xBegin; x < xEnd; ++x, ++ptr) {
					accumulation += *ptr;
					count++;
				}
			}
		}
		if (count == 0) return 0;
		return accumulation / count;
	}

	template <typename _Type>
	int calcInliersCount(const cv::Mat& inputMat, const AsMathPlane& mathPlane, double inliersThresh, const MatrixParams<_Type>& matrixParams) {
		int xBegin, yBegin, xEnd, yEnd;
		getRange(inputMat, matrixParams.roi, xBegin, yBegin, xEnd, yEnd);
		const _Type filterZLow = matrixParams.filterZLow;
		const _Type filterZUp = matrixParams.filterZUp;
		int count = 0;
		const _Type* ptr;
		if (matrixParams.enableZFilter) {
			for (int y = yBegin; y < yEnd; ++y) {
				ptr = inputMat.ptr<_Type>(y, xBegin);
				for (int x = xBegin; x < xEnd; ++x, ++ptr) {
					_Type value = *ptr;
					if (value >= filterZLow && value <= filterZUp) {
						double dist = mathPlane.calcDistance(cv::Point3f(x, y, value));
						if (dist < inliersThresh) {
							count++;
						}
					}
				}
			}
		}
		else {
			for (int y = yBegin; y < yEnd; ++y) {
				ptr = inputMat.ptr<_Type>(y, xBegin);
				for (int x = xBegin; x < xEnd; ++x, ++ptr) {
					double dist = mathPlane.calcDistance(cv::Point3f(x, y, *ptr));
					if (dist < inliersThresh) {
						count++;
					}
				}
			}
		}
		return count;
	}

	template <typename _Type>
	void hfusion(cv::Mat& inputDstMat, const cv::Mat& inputApendMat, int overlapLength) {
		if (inputDstMat.rows != inputApendMat.rows) return;
		cv::Mat workMat1 = shortenToRight(inputDstMat, inputDstMat.cols - overlapLength);
		cv::Mat workMat2 = shortenToLeft(inputApendMat, inputApendMat.cols - overlapLength);
		const int rows = workMat1.rows;
		const int cols = workMat1.cols;
		_Type* ptr1;
		const _Type* ptr2;
		for (int row = 0; row < rows; ++row) {
			ptr1 = workMat1.ptr<_Type>(row);
			ptr2 = workMat2.ptr<_Type>(row);
			for (int col = 0; col < cols; ++col, ++ptr1, ++ptr2) {
				float ratio = (float)col / cols;
				*ptr1 = *ptr1 * (1 - ratio) + *ptr2 * (ratio);
			}
		}
		std::vector<cv::Mat> mats;
		mats.push_back(inputDstMat);
		mats.push_back(shortenToRight(inputApendMat, overlapLength));
		cv::hconcat(mats, inputDstMat);
	}

	template <typename _Type>
	void hfusion_concat(const std::vector<cv::Mat>& inputMats, cv::Mat& outputMat, int overlapLength) {
		if (inputMats.empty()) return;
		outputMat = inputMats.front();
		for (int i = 1; i < inputMats.size(); ++i) {
			hfusion<_Type>(outputMat, inputMats[i], overlapLength);
		}
	}

	int AS_MATH_API calcMode(const cv::Mat& inputMat, cv::Rect roi = cv::Rect(-1, -1, -1, -1));
	int AS_MATH_API calcMode(const cv::Mat& inputMat, uchar pixLow, uchar pixUp, cv::Rect roi = cv::Rect(-1, -1, -1, -1));

};

namespace AsMath {

	template <typename _Type>
	std::vector<cv::Point_<_Type>> matRow2XZPoints(const cv::Mat& inputMat, int row) {
		const _Type* ptr = inputMat.ptr<_Type>(row, 0);
		const int cols = inputMat.cols;
		std::vector<cv::Point_<_Type>> outputPoints;
		outputPoints.reserve(cols);
		for (int x = 0; x < cols; ++x, ++ptr) {
			outputPoints.emplace_back(cv::Point_<_Type>(x, *ptr));
		}
		return std::move(outputPoints);
	}

	template <typename _Type>
	std::vector<cv::Point_<_Type>> matRow2XZPoints(const cv::Mat& inputMat, int row, _Type filterZLow, _Type filterZUp) {
		const _Type* ptr = inputMat.ptr<_Type>(row, 0);
		const int cols = inputMat.cols;
		std::vector<cv::Point_<_Type>> outputPoints;
		outputPoints.reserve(cols);
		for (int x = 0; x < cols; ++x, ++ptr) {
			if (*ptr >= filterZLow && *ptr <= filterZUp) {
				outputPoints.emplace_back(cv::Point_<_Type>(x, *ptr));
			}
		}
		return std::move(outputPoints);
	}

	template <typename _Type>
	std::vector<cv::Point_<_Type>> matColumn2YZPoints(const cv::Mat& inputMat, int column) {
		const int rows = inputMat.rows;
		std::vector<cv::Point_<_Type>> outputPoints;
		outputPoints.reserve(rows);
		for (int y = 0; y < rows; ++y) {
			outputPoints.emplace_back(cv::Point_<_Type>(y, *inputMat.ptr<_Type>(y, column)));
		}
		return std::move(outputPoints);
	}

	template <typename _Type>
	std::vector<cv::Point_<_Type>> matColumn2YZPoints(const cv::Mat& inputMat, int column, _Type filterZLow, _Type filterZUp) {
		const int rows = inputMat.rows;
		std::vector<cv::Point_<_Type>> outputPoints;
		outputPoints.reserve(rows);
		for (int y = 0; y < rows; ++y) {
			const _Type z = *inputMat.ptr<_Type>(y, column);
			if (z >= filterZLow && z <= filterZUp) {
				outputPoints.emplace_back(cv::Point_<_Type>(y, z));
			}
		}
		return std::move(outputPoints);
	}

	template <typename _Type>
	std::vector<cv::Point3_<_Type>> mat2XYZPoints(const cv::Mat& inputMat, _Type filterZLow, _Type filterZUp) {
		const int rows = inputMat.rows;
		const int cols = inputMat.cols;
		std::vector<cv::Point3_<_Type>> outputPoints;
		outputPoints.reserve(rows * cols);
		for (int y = 0; y < rows; ++y) {
			const _Type* ptr = inputMat.ptr<_Type>(y, 0);
			for (int x = 0; x < cols; ++x, ++ptr) {
				if (*ptr >= filterZLow && *ptr <= filterZUp) {
					outputPoints.emplace_back(cv::Point3_<_Type>(x, y, *ptr));
				}
			}
		}
		return std::move(outputPoints);
	}

};

